package com.google.android.apps.skipbot;

import android.os.AsyncTask;

/**
 * Class that implements a robot movement sequence for moving forward using a
 * ripple.
 */
public class SkipperRippleGaitSequence extends SkipperMovementSequence {

  private static final int NUMBER_OF_STEPS = 10;
  private static final int SHORT_PAUSE = 25;
  private static final int LONG_PAUSE = 250;

  /**
   * Creates a ripple gait sequence for the Skipper robot.
   * 
   * @param robotConfig robot configuration
   * @param robotController robot controller that enables robot movements
   */
  public SkipperRippleGaitSequence(SkipperRobotConfig robotConfig,
      SkipperRobotController robotController) {
    super(robotConfig, robotController);
  }

  @Override
  public void run(AsyncTask<RobotMovementSequence, Integer, Boolean> asyncTask) {
    try {
      for (int i = 0; i < NUMBER_OF_STEPS; ++i) {
        // Lift the first leg
        robotController.moveServo(ELBOW_0, highStepAngle[ELBOW_0]);
        Thread.sleep(SHORT_PAUSE);

        robotController.moveServo(SHOULDER_0, highStepAngle[SHOULDER_0]);
        Thread.sleep(LONG_PAUSE);

        // Rotate the hip
        robotController.moveServo(HIP_0, 105);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_0, standAngle[ELBOW_0]);
        robotController.moveServo(SHOULDER_0, standAngle[SHOULDER_0]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_1, highStepAngle[ELBOW_1]);
        Thread.sleep(SHORT_PAUSE);

        robotController.moveServo(SHOULDER_1, highStepAngle[SHOULDER_1]);
        Thread.sleep(LONG_PAUSE);

        // Rotate the hip
        robotController.moveServo(HIP_1, 105);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_1, standAngle[ELBOW_1]);
        robotController.moveServo(SHOULDER_1, standAngle[SHOULDER_1]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_2, highStepAngle[ELBOW_2]);
        Thread.sleep(SHORT_PAUSE);

        robotController.moveServo(SHOULDER_2, highStepAngle[SHOULDER_2]);
        Thread.sleep(LONG_PAUSE);

        // Rotate the hip
        robotController.moveServo(HIP_2, 105);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_2, standAngle[ELBOW_2]);
        robotController.moveServo(SHOULDER_2, standAngle[SHOULDER_2]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_3, highStepAngle[ELBOW_3]);
        Thread.sleep(SHORT_PAUSE);

        robotController.moveServo(SHOULDER_3, highStepAngle[SHOULDER_3]);
        Thread.sleep(LONG_PAUSE);

        // Rotate the hip
        robotController.moveServo(HIP_3, 75);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_3, standAngle[ELBOW_3]);
        robotController.moveServo(SHOULDER_3, standAngle[SHOULDER_3]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_4, highStepAngle[ELBOW_4]);
        Thread.sleep(SHORT_PAUSE);

        robotController.moveServo(SHOULDER_4, highStepAngle[SHOULDER_4]);
        Thread.sleep(LONG_PAUSE);

        // Rotate the hip
        robotController.moveServo(HIP_4, 75);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_4, standAngle[ELBOW_4]);
        robotController.moveServo(SHOULDER_4, standAngle[SHOULDER_4]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_5, highStepAngle[ELBOW_5]);
        Thread.sleep(SHORT_PAUSE);

        robotController.moveServo(SHOULDER_5, highStepAngle[SHOULDER_5]);
        Thread.sleep(LONG_PAUSE);

        // Rotate the hip
        robotController.moveServo(HIP_5, 75);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(ELBOW_5, standAngle[ELBOW_5]);
        robotController.moveServo(SHOULDER_5, standAngle[SHOULDER_5]);
        Thread.sleep(3 * LONG_PAUSE);

        robotController.moveServo(SHOULDER_1, highStepAngle[SHOULDER_1]);
        robotController.moveServo(SHOULDER_3, highStepAngle[SHOULDER_3]);
        robotController.moveServo(SHOULDER_5, highStepAngle[SHOULDER_5]);
        robotController.moveServo(ELBOW_1, highStepAngle[ELBOW_1]);
        robotController.moveServo(ELBOW_3, highStepAngle[ELBOW_3]);
        robotController.moveServo(ELBOW_5, highStepAngle[ELBOW_5]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(HIP_0, standAngle[HIP_0]);
        robotController.moveServo(HIP_2, standAngle[HIP_2]);
        robotController.moveServo(HIP_4, standAngle[HIP_4]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(SHOULDER_1, standAngle[SHOULDER_1]);
        robotController.moveServo(SHOULDER_3, standAngle[SHOULDER_3]);
        robotController.moveServo(SHOULDER_5, standAngle[SHOULDER_5]);
        robotController.moveServo(ELBOW_1, standAngle[ELBOW_1]);
        robotController.moveServo(ELBOW_3, standAngle[ELBOW_3]);
        robotController.moveServo(ELBOW_5, standAngle[ELBOW_5]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(SHOULDER_4, highStepAngle[SHOULDER_4]);
        robotController.moveServo(SHOULDER_2, highStepAngle[SHOULDER_2]);
        robotController.moveServo(SHOULDER_0, highStepAngle[SHOULDER_0]);
        robotController.moveServo(ELBOW_4, highStepAngle[ELBOW_4]);
        robotController.moveServo(ELBOW_2, highStepAngle[ELBOW_2]);
        robotController.moveServo(ELBOW_0, highStepAngle[ELBOW_0]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(HIP_1, standAngle[HIP_1]);
        robotController.moveServo(HIP_3, standAngle[HIP_3]);
        robotController.moveServo(HIP_5, standAngle[HIP_5]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(SHOULDER_4, standAngle[SHOULDER_4]);
        robotController.moveServo(SHOULDER_2, standAngle[SHOULDER_2]);
        robotController.moveServo(SHOULDER_0, standAngle[SHOULDER_0]);
        robotController.moveServo(ELBOW_4, standAngle[ELBOW_4]);
        robotController.moveServo(ELBOW_2, standAngle[ELBOW_2]);
        robotController.moveServo(ELBOW_0, standAngle[ELBOW_0]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(SHOULDER_1, highStepAngle[SHOULDER_1]);
        robotController.moveServo(SHOULDER_3, highStepAngle[SHOULDER_3]);
        robotController.moveServo(SHOULDER_5, highStepAngle[SHOULDER_5]);
        robotController.moveServo(ELBOW_1, highStepAngle[ELBOW_1]);
        robotController.moveServo(ELBOW_3, highStepAngle[ELBOW_3]);
        robotController.moveServo(ELBOW_5, highStepAngle[ELBOW_5]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(HIP_0, 75);
        robotController.moveServo(HIP_2, 75);
        robotController.moveServo(HIP_4, 105);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(SHOULDER_1, standAngle[SHOULDER_1]);
        robotController.moveServo(SHOULDER_3, standAngle[SHOULDER_3]);
        robotController.moveServo(SHOULDER_5, standAngle[SHOULDER_5]);
        robotController.moveServo(ELBOW_1, standAngle[ELBOW_1]);
        robotController.moveServo(ELBOW_3, standAngle[ELBOW_3]);
        robotController.moveServo(ELBOW_5, standAngle[ELBOW_5]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(SHOULDER_4, highStepAngle[SHOULDER_4]);
        robotController.moveServo(SHOULDER_2, highStepAngle[SHOULDER_2]);
        robotController.moveServo(SHOULDER_0, highStepAngle[SHOULDER_0]);
        robotController.moveServo(ELBOW_4, highStepAngle[ELBOW_4]);
        robotController.moveServo(ELBOW_2, highStepAngle[ELBOW_2]);
        robotController.moveServo(ELBOW_0, highStepAngle[ELBOW_0]);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(HIP_1, 75);
        robotController.moveServo(HIP_3, 105);
        robotController.moveServo(HIP_5, 105);
        Thread.sleep(LONG_PAUSE);

        robotController.moveServo(SHOULDER_4, standAngle[SHOULDER_4]);
        robotController.moveServo(SHOULDER_2, standAngle[SHOULDER_2]);
        robotController.moveServo(SHOULDER_0, standAngle[SHOULDER_0]);
        robotController.moveServo(ELBOW_4, standAngle[ELBOW_4]);
        robotController.moveServo(ELBOW_2, standAngle[ELBOW_2]);
        robotController.moveServo(ELBOW_0, standAngle[ELBOW_0]);
        Thread.sleep(LONG_PAUSE);
      }
      Thread.sleep(LONG_PAUSE);
    } catch (InterruptedException ignore) {
      return;
    } finally {
      // TODO(bbrown): Move into a known good stop position.
    }
  }

}
